#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>

//#include<cv_bridge/cv_bridge.h>
#include "cv_bridge/cv_bridge.h"
#include<opencv2/opencv.hpp>
#include <nav_msgs/Odometry.h>

//#include <pcl/conversions.h>

#include <map>

#include <pcl/filters/crop_box.h>
#include<bits/stdc++.h>

using namespace Eigen;
using namespace std;
using PointType = pcl::PointXYZINormal;
ros::Publisher pc_erasur_pub_,pc_removert_pub_,pc_ours_pub_,pc_origin_pub_;

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "mcl_3dl");
    ros::NodeHandle nh;

    pc_erasur_pub_ = nh.advertise<sensor_msgs::PointCloud2>("pc_erasur",10);
    pc_removert_pub_ = nh.advertise<sensor_msgs::PointCloud2>("pc_removert",10);
    pc_ours_pub_ = nh.advertise<sensor_msgs::PointCloud2>("pc_ours",10);
    pc_origin_pub_ = nh.advertise<sensor_msgs::PointCloud2>("pc_origin",10);

    pcl::PointCloud<PointType>::Ptr pc_erasur(new pcl::PointCloud<PointType>),
        pc_removert(new pcl::PointCloud<PointType>),pc_ours(new pcl::PointCloud<PointType>),pc_origin(new pcl::PointCloud<PointType>);
    pcl::io::loadPCDFile<PointType>("/home/st/ubuntu_data/dataset/keti/envalute/2022_02/ESCUR/bongeunsa_result.pcd", *pc_erasur);
    pcl::io::loadPCDFile<PointType>("/home/st/ubuntu_data/dataset/keti/envalute/2022_02/StaticMapMapsideGlobalResX1.100000.pcd", *pc_removert);
    pcl::io::loadPCDFile<PointType>("/home/st/ubuntu_data/dataset/keti/envalute/2022_02/Ours/static_map.pcd", *pc_ours);
    pcl::io::loadPCDFile<PointType>("/home/st/ubuntu_data/dataset/keti/envalute/2022_02/gt_map.pcd", *pc_origin);

    ros::Rate rate(0.1);
    while(ros::ok()){
        //publish
        sensor_msgs::PointCloud2 tempCloud;
        pcl::toROSMsg(*pc_erasur, tempCloud);
        tempCloud.header.stamp = ros::Time::now();
        tempCloud.header.frame_id = "map";
        pc_erasur_pub_.publish(tempCloud);
        pcl::toROSMsg(*pc_removert, tempCloud);
        tempCloud.header.stamp = ros::Time::now();
        tempCloud.header.frame_id = "map";
        pc_removert_pub_.publish(tempCloud);
        pcl::toROSMsg(*pc_ours, tempCloud);
        tempCloud.header.stamp = ros::Time::now();
        tempCloud.header.frame_id = "map";
        pc_ours_pub_.publish(tempCloud);
        pcl::toROSMsg(*pc_origin, tempCloud);
        tempCloud.header.stamp = ros::Time::now();
        tempCloud.header.frame_id = "map";
        pc_origin_pub_.publish(tempCloud);
        rate.sleep();
    }
   
    return 0;
}
